/*
 * servo.c
 *
 *  Created on: 11.2.2012
 *      Author: lukas
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include "servo.h"

static uint8_t APPLY_BOUNDARIES = 1;

/*
 * Prescaler is 64 assuming that the frequency of the processor is 8 000 000 Hz
 * Using timer1 (16bit) in Fast PWM mode
 *
 */
void initServo(uint8_t applyBoundaries) {
	APPLY_BOUNDARIES = applyBoundaries;
	cli();
	DDRD |= (1 << PIND5) | (1 << PIND4);
	TCCR1A = (1 << WGM11) | (1 << COM1A1);
	TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS10) | (1 << CS11);
	ICR1 = TOP_VALUE;
	OCR1A = NEUTRAL_POSITION;
	sei();
}

/*
 * Sets the position of the servomotor
 * mind that values are added 100 before applying to fit into one byte
 * if APPLY_BOUNDARIES values will be checked for max and min value
 * if not APPLY_BOUNDARIES you can damage the servo using wrong values
 */
void positionServo(uint8_t position) {
	uint16_t recalculatedPosition = 100 + position;
	if (APPLY_BOUNDARIES) {
		if (recalculatedPosition < MIN_VALUE) {
			recalculatedPosition = MIN_VALUE;
		} else if (recalculatedPosition > MAX_VALUE) {
			recalculatedPosition = MAX_VALUE;
		}
	}
	OCR1A = recalculatedPosition;
}

/*
 * Returns current position of the servomotor
 * This value is corrected -100 to fit into one byte
 */
char getCurrentServoPosition() {
	return (char) OCR1A - 100;
}


